/** ###################################################################
**     Filename  : linear_ccd.c
**     Project   : ccd2
**     Processor : MCF52255CAF80
**     Version   : Driver 01.00
**     Compiler  : CodeWarrior MCF C Compiler
**     Arthor	 : Yumi Chan, Cindy Yu, Ryan Cheung
**     Abstract  :
**         
**     Settings  :
**     Contents  :
**         No public methods
**
** ###################################################################*/

/* Include */
#include "linear_ccd.h"


/* Variables */
LINEAR_CCD ccd[MAX_NUM_OF_CCD];

#if (USE_TWO_LINEAR_CCD)
float trackWidth_ratio = 0;
#endif


/* Private Method */
void collectCCDImage() 
{
	uint8 i;

	CCD_SI_SetVal();
	CCD2_SI_SetVal();
	delay_us(5);
	
	CCD_CLK_SetVal();
	CCD2_CLK_SetVal();
	delay_us(5);
	
	CCD_SI_ClrVal();
	CCD2_SI_ClrVal();
	delay_us(5);
	
	/* Remove 4 pixels on left side */
	CCD_CLK_ClrVal();
	CCD2_CLK_ClrVal();
	CCD_CLK_SetVal();
	CCD2_CLK_SetVal();
	delay_us(5);
	
	CCD_CLK_ClrVal();
	CCD2_CLK_ClrVal();
	CCD_CLK_SetVal();
	CCD2_CLK_SetVal();
	delay_us(5);
	
	CCD_CLK_ClrVal();
	CCD2_CLK_ClrVal();
	CCD_CLK_SetVal();
	CCD2_CLK_SetVal();
	delay_us(5);
	
	CCD_CLK_ClrVal();
	CCD2_CLK_ClrVal();
	CCD_CLK_SetVal();
	CCD2_CLK_SetVal();
	delay_us(5);

	for(i=0; i<IMAGE_RESOLUTION; ++i)
	{
		CCD_CLK_ClrVal();
		CCD2_CLK_ClrVal();
		delay_us(12);
		
		ccd[CCD1].image[i] = CCD_pixel_GetVal();
		//ccd[CCD1].image[i] = (ADonce(0) >> 4) + 50;
		ccd[CCD2].image[i] = CCD2_pixel_GetVal();
		//ccd[CCD2].image[i] = ADonce(0) >> 4;
		
		CCD_CLK_SetVal();
		CCD2_CLK_SetVal();
		delay_us(12);
	}
	
	/* Remove 4 pixels on right side */
	CCD_CLK_ClrVal();
	CCD2_CLK_ClrVal();
	CCD_CLK_SetVal();
	CCD2_CLK_SetVal();
	delay_us(5);
	
	CCD_CLK_ClrVal();
	CCD2_CLK_ClrVal();
	CCD_CLK_SetVal();
	CCD2_CLK_SetVal();
	delay_us(5);
	
	CCD_CLK_ClrVal();
	CCD2_CLK_ClrVal();
	CCD_CLK_SetVal();
	CCD2_CLK_SetVal();
	delay_us(5);
	
	CCD_CLK_ClrVal();
	CCD2_CLK_ClrVal();
	CCD_CLK_SetVal();
	CCD2_CLK_SetVal();
	delay_us(5);

	CCD_CLK_ClrVal();
	CCD2_CLK_ClrVal();
	return ;
}

void findTrackWidth() 
{
  short i;
  byte no_of_ccd_used = 0;
  byte j=0;
  
#if (USE_TWO_LINEAR_CCD)
  no_of_ccd_used = 2;
#else
  no_of_ccd_used = 1;
#endif

  for (; j<no_of_ccd_used; ++j)
  {
  	ccd[j].left_edge_pos[CURRENT] = 0;
  	ccd[j].right_edge_pos[CURRENT] = 0;
  	for (i=servoPID.set_value; i >= 0; --i)
  	{
  		if (ccd[j].image[i+1] == WHITE && ccd[j].image[i] == BLACK && ccd[j].image[i-1] == BLACK)
  		{
  			ccd[j].left_edge_pos[CURRENT] = i;
  			break;
  		}
  	}
  	
  	for (i=servoPID.set_value; i < IMAGE_RESOLUTION; ++i)
  	{
  		if (ccd[j].image[i-1] == WHITE && ccd[j].image[i] == BLACK && ccd[j].image[i+1] == BLACK)
  		{
  			ccd[j].right_edge_pos[CURRENT] = i;
  			break;
  		}
  	}
  ccd[j].trackWidth = ccd[j].right_edge_pos[CURRENT] - ccd[j].left_edge_pos[CURRENT];
  ccd[j].trackCenter = (ccd[j].left_edge_pos[CURRENT] + ccd[j].right_edge_pos[CURRENT])/2;
  }
  
#if (USE_TWO_LINEAR_CCD)
  if (ccd[CCD2].trackWidth != 0)
  {
  	trackWidth_ratio = ccd[CCD1].trackWidth / ccd[CCD2].trackWidth;	
  }
  
#endif
}
